#include "Body.h"

#include <iostream>
#include <algorithm>
#include <cmath>

#include <GL/glew.h>
#include <GL/glut.h>

extern float* particleColorData;
extern float* particleVertexData;
extern float terminalVelocity;

int Body::count = 0;

Body::Body() {

	mass = 0.2f;
	for (int i=0; i<3; i++) {
		color[i] = (float)rand() / (float)RAND_MAX;
	} 
	terminalMomentum = terminalVelocity*mass;

	position = {0.0f, 0.0f, 0.0f};
	force = {0.0f, 0.0f, 0.0f};
	linearMomentum = {0.0f, 0.0f, 0.0f};
	angularMomentum = {0.0f, 0.0f, 0.0f};
	quaternion = {0.0f, 1.0f, 0.0f, 0.0f}; 

	initialInverseInertiaTensorDiagonal = {0.0f, 0.0f, 0.0f};

	inverseInertiaTensor = {0.0f, 0.0f, 0.0f,
			0.0f, 0.0f, 0.0f,
			0.0f, 0.0f, 0.0f};

	count++;
}

void Body::performLinearStep(float delta){

	for(int i=0; i<3; i++) {

		velocity[i] = linearMomentum[i] / mass;
		position[i] = position[i] + velocity[i]*delta;
	}
}

void Body::performAngularStep(float delta) {
	{ //update angular velocity
		float a = inverseInertiaTensor[0];
		float b = inverseInertiaTensor[1];
		float c = inverseInertiaTensor[2];
		float d = inverseInertiaTensor[3];
		float e = inverseInertiaTensor[4];
		float f = inverseInertiaTensor[5];
		float g = inverseInertiaTensor[6];
		float h = inverseInertiaTensor[7];
		float i = inverseInertiaTensor[8];

		float u = angularMomentum[0];
		float v = angularMomentum[1];
		float w = angularMomentum[2];

		angularVelocity[0] = a*u + b*v + c*w;
		angularVelocity[1] = d*u + e*v + f*w;
		angularVelocity[2] = g*u + h*v + i*w;
	}
	float angularVelocitySize = sqrt(angularVelocity[0]*angularVelocity[0] +
		angularVelocity[1]*angularVelocity[1] +
		angularVelocity[2]*angularVelocity[2]); 

	if (angularVelocitySize > 0) {

		float rotationAxis[3] = {angularVelocity[0]/angularVelocitySize,
			angularVelocity[1]/angularVelocitySize,
			angularVelocity[2]/angularVelocitySize};

		float rotationAngle = angularVelocitySize*delta;

		float ds = cos(rotationAngle/2.0f);
		float dvx = rotationAxis[0]*sin(rotationAngle/2.0f);
		float dvy = rotationAxis[1]*sin(rotationAngle/2.0f);
		float dvz = rotationAxis[2]*sin(rotationAngle/2.0f);

		float s = quaternion[0];
		float vx = quaternion[1];
		float vy = quaternion[2];
		float vz = quaternion[3];

		quaternion[0] = s*ds - vx*dvx - vy*dvy - vz*dvz;
		quaternion[1] = ds*vx + s*dvx + dvy*vz - dvz*vy;
		quaternion[2] = ds*vy + s*dvy + dvz*vx - dvx*vz;
		quaternion[3] = ds*vz + s*dvz + dvx*vy - dvy*vx;
	}
}

void Body::updateParticleValues() {
	updateRotationMatrix();
	applyRotationToParticles();

	//Update particle velocity
	for (int i=0; i<numberOfParticles; i++) {
		particles[i]->updateVelocity(position, velocity, angularVelocity);
	}
}

void Body::performStep(float delta) {
	updateInverseInertiaTensor();
	performLinearStep(delta);
	performAngularStep(delta);
}

void Body::updateParticleVBO(int vertexIndex) {
	for (int i=0; i<numberOfParticles; i++) {
		float* particlePos = particles[i]->getPosition();

		particleVertexData[vertexIndex] = particlePos[0];
		particleVertexData[vertexIndex+1] = particlePos[1];
		particleVertexData[vertexIndex+2] = particlePos[2];

		vertexIndex += 3;
	}
}

void Body::populateParticleArray() {
	for (int i=0; i<numberOfParticles; i++) {
		particles[i]->populateArray();
	}
}

void Body::updateRotationMatrix() {

	normalizeQuaternion();

	float w = quaternion[0];
	float x = quaternion[1];
	float y = quaternion[2];
	float z = quaternion[3];

	float xx = x * x;
	float yy = y * y;
	float zz = z * z;
	float xy = x * y;
	float xz = x * z;
	float yz = y * z;
	float wx = w * x;
	float wy = w * y;
	float wz = w * z; 

	rotationMatrix = { 1.0f-2.0f*(yy+zz), 2.0f*(xy-wz), 2.0f*(xz+wy), 
			   2.0f*(xy+wz), 1.0f-2.0f*(xx+zz), 2.0f*(yz-wx),
			   2.0f*(xz-wy), 2.0f*(yz+wx), 1.0f-2.0f*(xx+yy) };
}

void Body::normalizeQuaternion() {
	float mag2 = quaternion[0]*quaternion[0] +
		quaternion[1]*quaternion[1] +
		quaternion[2]*quaternion[2] +
		quaternion[3]*quaternion[3];

	if (mag2!=0.0f && (fabs(mag2 - 1.0f) > 0.00001f)) {
		float mag = sqrt(mag2);
		quaternion[0] /= mag;
		quaternion[1] /= mag;
		quaternion[2] /= mag;
		quaternion[3] /= mag;
	}
}

void Body::updateMomenta(float delta) {
	force = {0.0f, 0.0f, 0.0f}; //reset forces
	force[1] = force[1] + mass * -9.81f; //force of gravity

	float torque[3] = {0.0f, 0.0f, 0.0f};

	for (int i=0; i<numberOfParticles; i++) {
		float* particleForce = particles[i]->calculateForces();
		force[0] += particleForce[0];
		force[1] += particleForce[1];
		force[2] += particleForce[2];

		float* particlePos = particles[i]->getPosition();
		float relativePos[3];
		relativePos[0] = particlePos[0] - position[0];
		relativePos[1] = particlePos[1] - position[1];
		relativePos[2] = particlePos[2] - position[2];

		torque[0] += relativePos[1]*particleForce[2] - relativePos[2]*particleForce[1];
		torque[1] += relativePos[2]*particleForce[0] - relativePos[0]*particleForce[2];
		torque[2] += relativePos[0]*particleForce[1] - relativePos[1]*particleForce[0];
	}

	for (int i=0; i<3; i++) {

		linearMomentum[i] += force[i] * delta;
		if (linearMomentum[i] > 0.0f) {
			linearMomentum[i] = std::min(linearMomentum[i],terminalMomentum);	
		} else {
			linearMomentum[i] = std::max(linearMomentum[i],-terminalMomentum);
		}

		angularMomentum[i] += torque[i] * delta;
	}
}

void Body::reset(float* newPosition) {

	for (int i=0; i<numberOfParticles; i++) {
		particles[i]->reset(position, newPosition);
	}

	position[0] = newPosition[0];
	position[1] = newPosition[1];
	position[2] = newPosition[2];

	linearMomentum = {0.0f, 0.0f, 0.0f};
	angularMomentum = {0.0f, 0.0f, 0.0f};
	velocity = {0.0f, 0.0f, 0.0f};
	angularVelocity = {0.0f, 0.0f, 0.0f};

	quaternion = {0.0f, 1.0f, 0.0f, 0.0f}; 
	
	updateRotationMatrix();
	applyRotationToParticles();
	
}

void Body::updateParticleColorArray(int vertexIndex) {
	for (int i=0; i<numberOfParticles; i++) {
		particleColorData[vertexIndex] = color[0];
		particleColorData[vertexIndex+1] = color[1];
		particleColorData[vertexIndex+2] = color[2];

		vertexIndex += 3;
	}
}
